#include "rtcmds.h"
#include "command.h"
#include "zuc/mot_cfg.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "log/zuclog.h"
#include "axisgroup/robot_mgr.h"

#define MOTSERVER_INFO(fmt, ...) zuclog_info("RTCMD_SVR", fmt, ##__VA_ARGS__)
#define MOTSERVER_ERR(fmt, ...) zuclog_error("RTCMD_SVR", fmt, ##__VA_ARGS__)
#define MOTSERVER_DEBUG(fmt, ...) zuclog_debug("RTCMD_SVR", fmt, ##__VA_ARGS__)

#define PUSH_CMD_TIMEOUT_MS 40

static long long get_time_ms(void)
{
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return (ts.tv_sec * 1000000000LL + ts.tv_nsec) / 1000000;
}

using namespace mot;
static RtCmdSvr mot_cmd_svr;
int motion_cmd_svr()
{
    mot_cmd_svr.update();
    return 0;
}
int mot::realtime_cmd_call(std::shared_ptr<mot::RtCmdItf> cmd) { return mot_cmd_svr.call(cmd); }

RtCmdSvr::RtCmdSvr() {}
int RtCmdSvr::call(std::shared_ptr<RtCmdItf> cmd)
{
    if (cmd_list_.size() >= MOTCMD_MAX_CNT)
    {
        uint64_t cur_time = get_time_ms();
        uint64_t dt = 0;
        while (cmd_list_.size() >= MOTCMD_MAX_CNT)
        {
            usleep(2000);  // 等待
            dt = (get_time_ms()) - cur_time;
            if (dt > PUSH_CMD_TIMEOUT_MS)
            {
                MOTSERVER_ERR("Cmd list is full, drop cmd %s", cmd->cmd_name_.c_str());
                return -2;
            }
        }
        MOTSERVER_DEBUG("Call cmd %s wait %d ms", cmd->cmd_name_.c_str(), dt);
    }
    if (cmd->active())
    {
        MOTSERVER_DEBUG("active_callback cmd %s failed", cmd->cmd_name_.c_str());
        return -3;
    }
    std::lock_guard<std::mutex> lock(mtx_);
    cmd_list_.push_back(cmd);
    MOTSERVER_DEBUG("Call cmd %s", cmd->cmd_name_.c_str());
    return 0;
}

void RtCmdSvr::update()
{
    std::list<std::shared_ptr<RtCmdItf>> cur_insts;
    {
        std::lock_guard<std::mutex> lock(mtx_);
        if (cmd_list_.empty())
        {
            return;
        }
        auto cmd_cnt = cmd_list_.size();
        for (size_t i = 0; i < cmd_cnt; i++)
        {
            auto cmd = cmd_list_.front();
            cmd_list_.pop_front();
            cur_insts.push_back(cmd);
        }
    }
    for (auto& cmd : cur_insts)
    {
        MOTSERVER_DEBUG("Process cmd %s", cmd->cmd_name_.c_str());
        if (cmd->process())
        {
            MOTSERVER_ERR("Cmd %s process failed", cmd->cmd_name_.c_str());
        }
    }
}

int mot::MultiRobCmd::active()
{
    if (!axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_).get())
    {
        MOTSERVER_ERR("Group %d not exist", group_id_);
        return -1;
    }
    return active_callback();
}
